from maix import camera , display , image , pinmap , gpio , app,time ,nn,uart
import numpy as np 
import cv2 , struct
import math

kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(3,3))
cam = camera.Camera(320,240) #初始化相机
disp = display.Display() #初始化显示屏

'''创建按键对象'''
pinmap.set_pin_function("A28","GPIOA28")
pinmap.set_pin_function("A29","GPIOA29")
pinmap.set_pin_function("A19","GPIOA19")
pinmap.set_pin_function("A18","GPIOA18")
key1 = gpio.GPIO("GPIOA19",gpio.Mode.IN)
key2 = gpio.GPIO("GPIOA18",gpio.Mode.IN)
key3 = gpio.GPIO("GPIOA29",gpio.Mode.IN)
key4 = gpio.GPIO("GPIOA28",gpio.Mode.IN)
pinmap.set_pin_function("B3", "GPIOB3")
led = gpio.GPIO("GPIOB3", gpio.Mode.OUT)

device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
serial = uart.UART(device1,115200)  #创建串口对象，八位数据，无检验，一位停止

last_cv2_img = None

kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(5,5))

run_task = 1

#flag:调试参数 0:调试 1:实际效果  mode:轮廓检测模式  0:检测外框 1:检测所有  number:检测轮廓的边长数量(0为检查所有边框)
def get_contour(mode=0, number=0, flag=0 ):
    points = []
    if mode == 0:
        find_mode = cv2.RETR_EXTERNAL
    else:
        find_mode = cv2.RETR_TREE

    cv_img_raw = image.image2cv(img) # 转cv
    cv_img = cv2.cvtColor(cv_img_raw, cv2.COLOR_BGR2GRAY) # 转灰度
    #cv_img = cv2.blur(cv_img,(4,4))
    #cv_img = cv2.bilateralFilter(cv_img,9,10,10)     # 双边滤波
    #cv_img = cv2.morphologyEx(cv_img,cv2.MORPH_CLOSE,kernel) # 闭运算
    cv_img = cv2.Canny(cv_img, 200, 300) # 边缘检测
    #cv_img = cv2.morphologyEx(cv_img, cv2.MORPH_CLOSE, np.ones((7,7), np.uint8))  # 闭合大裂缝
    #cv_img = cv2.dilate(cv_img, np.ones((5,5), np.uint8), iterations=1)          # 加粗边缘
    if flag:
        img_show = image.cv2image(cv_img)
        disp.show(img_show)
        print(111)
        return img_show, points  # 调试模式直接返回
    else:
        contours, hierarchy = cv2.findContours(cv_img, find_mode, cv2.CHAIN_APPROX_SIMPLE) # 找轮廓

        # 修复：正确解析层级结构
        if hierarchy is not None:
            hierarchy = hierarchy[0]  # 取出实际层级数组


        # 只处理有效轮廓（面积>=1000）
        valid_contours = []
        for contour in contours:
            area = cv2.contourArea(contour)
            if area < 1000:  # 过滤很小的轮廓
                continue
            valid_contours.append(contour)
        
        if not valid_contours:
            # 没有有效轮廓，直接返回
            return image.cv2image(cv_img_raw), points
            
        # 按面积排序并保留最大和最小
        areas = [(i, cv2.contourArea(contour)) for i, contour in enumerate(valid_contours)]
        sorted_areas = sorted(areas, key=lambda x: x[1])
        min_contour = valid_contours[sorted_areas[0][0]]  # 最小面积
        max_contour = valid_contours[sorted_areas[-1][0]]  # 最大面积
        # 获取最大和最小面积的轮廓索引
        min_idx = sorted_areas[0][0]
        max_idx = sorted_areas[-1][0]
        selected_contours = [min_contour, max_contour]


        for i, contour in enumerate(selected_contours):
            # 使用有效轮廓列表中的索引
            # 注意：现在i对应的是selected_contours的索引，而不是原contours
            epsilon = 0.02 * cv2.arcLength(contour, True)
            approx = cv2.approxPolyDP(contour, epsilon, True)
            
            if number != 0:
                # 跳过不符合顶点数量的轮廓
                if len(approx) != number:
                    continue

            if mode == 0:
                cv2.drawContours(cv_img_raw, [approx], 0, (0, 255, 0), 2)
                quad_points = []
                for point in approx:
                    x, y = point.ravel()
                    quad_points.append([x, y])
                    cv2.circle(cv_img_raw, (x, y), 5, (255, 0, 0), 1)
                points = quad_points  # 仅保留最后一个轮廓点
            else:
                # 根据层级或面积判断内外轮廓
                if i == max_idx:  # 最大面积默认为外轮廓
                    quad_points = []
                    for point in approx:
                        x, y = point.ravel()
                        quad_points.append([x, y])
                    points.append(quad_points)
                else:  # 最小面积默认为内轮廓
                    quad_points = []
                    for point in approx:
                        x, y = point.ravel()
                        quad_points.append([x, y])
                    points.append(quad_points)
                
                cv2.drawContours(cv_img_raw, [approx], 0, (0, 255, 0), 2)
                for point in approx:
                    x, y = point.ravel()
                    cv2.circle(cv_img_raw, (x, y), 5, (255, 0, 0), 1)

        img_show = image.cv2image(cv_img_raw)
        return img_show, points

#帧差法获取运动物体坐标
def get_sport():
    global last_cv2_img
    point_x = 5000
    point_y = 5000
    cv2_raw = image.image2cv(img)

    cv2_gray_img = cv2.cvtColor(cv2_raw , cv2.COLOR_BGR2GRAY) # 转灰度
    if last_cv2_img is None:
        last_cv2_img = cv2_gray_img.copy()

    #计算差值
    cv2_diff = cv2.absdiff(cv2_gray_img,last_cv2_img)

    #二值化
    _ , cv2_binary = cv2.threshold(cv2_diff , 25 ,255 , cv2.THRESH_BINARY)

    #膨胀处理
    cv2_binary = cv2.dilate(cv2_binary , kernel , iterations = 2)

    # 找轮廓
    contours , _ = cv2.findContours(cv2_binary,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    for contour in contours:
        #计算面积
        area = cv2.contourArea(contour)

        if area < 1000:
            print("已经找到物体")
            #用质心做运动物体的中心坐标
            M = cv2.moments(contour)
            point_x = int(M["m10"]/M["m00"])
            point_y = int(M["m01"]/M["m00"])
            img.draw_cross(point_x , point_y , image.COLOR_BLUE , 5,2)
            #获得轮廓的外接矩形
            x , y ,w ,h = cv2.boundingRect(contour)
            #获得该区域的直方图统计数据
            hist = img.get_histogram(thresholds= [[0,100,-128,127,-128,127]] , roi = [x,y,w,h])
            value = hist.get_statistics()

    last_cv2_img = cv2_gray_img.copy()
    return point_x , point_y

#串口发送函数 serial:串口实例  head:帧头   data:发送的数据
def uart_send(serial:uart.UART,head,data:list):
    send_data = struct.pack("<Bhh",head,data[0],data[1])
    serial.write(send_data)

def organize_nearest_pairs(outer_points, inner_points):
    """
    将外轮廓点与内轮廓点按最近邻原则配对，并返回中心点坐标
    
    参数:
        outer_points: 外轮廓点集 (例: [[171,77], [240,111], ...])
        inner_points: 内轮廓点集 (例: [[79,91], [73,163], ...])
    
    返回:
        中心点坐标列表: [[中心点x, 中心点y], ...]
    """
    # 计算两点间欧氏距离
    def calc_distance(point1, point2):
        return math.sqrt((point1[0]-point2[0])**2 + (point1[1]-point2[1])**2)

    center_points = []  # 存储中心点坐标
    used_indices = set()  # 记录已匹配的内点索引
    
    # 为每个外点寻找最近的内点
    for outer_pt in outer_points:
        min_dist = float('inf')
        nearest_inner = None
        nearest_idx = None  # 初始化为None，避免"possibly unbound"错误
        
        # 遍历所有内点找最近邻
        for idx, inner_pt in enumerate(inner_points):
            if idx in used_indices: 
                continue  # 跳过已匹配的点
                
            dist = calc_distance(outer_pt, inner_pt)
            if dist < min_dist:
                min_dist = dist
                nearest_inner = inner_pt
                nearest_idx = idx  # 更新最近点的索引
        
        # 计算中心点并保存
        if nearest_inner is not None and nearest_idx is not None:
            # 计算中心点坐标：((外点x + 内点x)/2, (外点y + 内点y)/2)
            center_x = int((outer_pt[0] + nearest_inner[0]) / 2)
            center_y = int((outer_pt[1] + nearest_inner[1]) / 2)
            center_points.append([center_x, center_y])
            used_indices.add(nearest_idx)
    
    return center_points

def traceing_point(target_point,fact_position):
    '''
    追踪定点坐标  

    参数:
         target_point:目标点坐标
         fact_position:实际点坐标

    返回:
        x轴误差 , y轴误差
    ''' 
    if target_point:
        ex = 0
        ey = 0
        if not fact_position :
            return (5000,5000)
        img.draw_keypoints([int(target_point[0]),int(target_point[1])],color=image.COLOR_YELLOW)
        # 计算当前误差
        ex = target_point[0] - fact_position[0]
        ey = target_point[1] - fact_position[1]
        ex = int(-ex)
        ey = int(-ey)
        return ex , ey
    else :
        return 0,0

def divide_polygon(vertices, n: int):
    """
    计算任意多边形各边的等分点（自动去重）
    
    参数:
        vertices: 多边形顶点坐标列表，按顺序排列 [(x1,y1), (x2,y2), ...]
        n: 每条边的等分段数（生成点数 = n）
    
    返回:
        等分点坐标列表: [[x1,y1], [x2,y2], ...]
    """
    if len(vertices) < 3:
        raise ValueError("多边形至少需要3个顶点")
    
    # 计算单条边的等分点（不包含终点）
    def divide_segment(start, end, segments: int):
        start_arr = np.array(start)
        end_arr = np.array(end)
        t = np.linspace(0, 1, segments + 1)[:-1]  # 排除终点
        points = start_arr + t[:, np.newaxis] * (end_arr - start_arr)
        return [p.astype(int).tolist() for p in points]
    
    all_points = []
    
    # 遍历每条边
    for i in range(len(vertices)):
        start = vertices[i]
        end = vertices[(i + 1) % len(vertices)]
        all_points.extend(divide_segment(start, end, n))
    
    # 添加多边形顶点
    all_points.extend([list(p) for p in vertices])
    
    # 去重（保留顺序）
    seen = {}
    unique_points = []
    for p in all_points:
        p_tuple = tuple(p)
        if p_tuple not in seen:
            seen[p_tuple] = True
            unique_points.append(p)
    
    return unique_points

def trace_polygon_loop(vertices, n, fact_position, threshold=5, current_index=0):
    """
    追踪多边形等分点并形成闭环追踪系统
    
    参数:
        vertices: 多边形顶点列表 [[x1,y1], [x2,y2], ...]
        n: 每条边的等分数
        fact_position: 实际点坐标 [x, y]
        threshold: 误差阈值(像素)，默认5
        current_index: 当前追踪点索引，默认0（首次调用）
    
    返回:
        (ex, ey, new_index, current_target, is_completed)
        ex, ey: x/y方向误差
        new_index: 更新后的目标点索引
    """
    # 生成多边形等分点
    target_points = divide_polygon(vertices, n)
    
    # 获取当前目标点
    current_target = target_points[current_index]
    
    # 计算当前误差
    ex, ey = traceing_point(current_target, fact_position)
    
    # 检查是否到达当前目标点
    if abs(ex) < threshold and abs(ey) < threshold:
        new_index = (current_index + 1) % len(target_points)  # 循环递增索引
    else:
        new_index = current_index

    
    return ex, ey, new_index 

def check_key():
    global run_task
    if not key1.value():
        print("按键一被按下")
        run_task = 1

    elif not key2.value():
        print("按键二被按下")
        run_task = 2
        
    elif not key3.value():
        print("按键三被按下")
        run_task = 3
        #red_laser.reset_task()
        
    elif not key4.value():
        print("按键四被按下")
        run_task = 4

tarpoint = [160,120]
point_index = 0
points = []
last_positon = [0,0]

while not app.need_exit():
    img = cam.read()
    ex = 0
    ey = 0
    
    check_key()

    if run_task == 1:
        img , points = get_contour(mode=1 , number=0)
        if len(points) == 2:
            points = organize_nearest_pairs(points[0],points[1])
    elif run_task == 2:
        position_x , position_y = get_sport()
        if position_x == 5000 and position_y == 5000:
            print("丢失目标")
            position_x = last_positon[0]
            position_y = last_positon[1]
        else:
            last_positon = [position_x , position_y]
        if points:
            ex ,ey , point_index = trace_polygon_loop(points , 20 , [position_x,position_y] , current_index=point_index)

    
    uart_send(serial,0xaa ,[ex, ey])
    disp.show(img)
    print(time.fps())